from bisect import bisect_left
from utils.namespace import Namespace
from utils import floatcompare as compare, MAXFLOAT, hsv2rgb

from cranesched.routing.geometry import Line
from cranesched.routing.path import Path


class BoundsManager(object):
    """This object is responsible for computing the lower and upper bounds for the movement of 
    each Movable1D object, taking their planned paths into account."""
    def __init__(self, lo=0.0, hi=100.0, time=0.0):
        self.limits = BoundsEntry(Namespace(size=0.0))
        self.limits.lb = Path(time, lo)
        self.limits.lb.append(MAXFLOAT, lo)
        self.limits.ub = Path(time, hi)
        self.limits.ub.append(MAXFLOAT, hi)
        self.entry = dict()     # {Movable1D: BoundsEntry}
        self.sequence = list()  # [Movable1D]
        self.stack = list()     # stack of saved entry mappings
        
    def set_limits(self, lo=None, hi=None, time=None):
        """Sets the "walls" between which the Movable1D objects can move."""
        if lo is not None:
            self.limits.lb.offset_position(lo - self.limits.lb.position)
        if hi is not None:
            self.limits.ub.offset_position(hi - self.limits.ub.position)
        if time is not None:
            self.limits.lb.offset_time(time - self.limits.lb.times[0])
            self.limits.ub.offset_time(time - self.limits.ub.times[0])
            
    def stretch_limits(self, factor):
        lo = self.limits.lb.position
        hi = self.limits.ub.position
        center = (lo + hi) * 0.5
        amplitude = (hi - lo) * factor
        self.set_limits(center - amplitude * 0.5, center + amplitude * 0.5)
        
    def add(self, movable):
        """Adds a Movable1D object to the bounds computer. The object is inserted into the 
        movables sequence, ordered by its position at t=0."""
        start_positions = [m.path.positions[0] for m in self.sequence]
        index = bisect_left(start_positions, movable.path.positions[0])
        self.sequence.insert(index, movable)
        
    def push(self):
        """Saves the current state of the bounds entry mapping into the stack."""
        self.stack.append(self.entry)
        self.clear()
        
    def pop(self):
        """Reverts the bounds entry mapping to the one that last saved."""
        self.entry = self.stack.pop()
        
    def clear(self):
        """Creates new (empty) bounds entries for all movables in the router."""
        self.entry = dict((movable, BoundsEntry(movable)) for movable in self.sequence)
        
    def clear_stack(self):
        """Forgets about previous bounds that were stored in the stack."""
        del self.stack[:]
        
    def update(self, force=False):
        """Computes movement lower and upper bounds for all Movable1D objects."""
        if force or len(self.entry) != len(self.sequence):
            self.clear()
        entry = self.entry
        below = self.limits
        for movable in self.sequence:
            current = entry[movable]
            current.compute_lb(below)
            below = current
        above = self.limits
        for movable in reversed(self.sequence):
            current = entry[movable]
            current.compute_ub(above)
            above = current
            
    def is_reachable(self, movable, target):
        """Return true if 'movable' can at some arbitrary point in time reach position 'target'."""
        entry = self.entry[movable]
        return (compare.geq(target, entry.lb.position) and 
                compare.leq(target, entry.ub.position))
        
    def show(self, target=None, title=None):
        for i, movable in enumerate(self.sequence):
            # compute a color for the movable, evenly distributed along the hue spectrum
            color = hsv2rgb(float(i) / len(self.sequence))
            # plot the planned path first
            target = movable.path.line_plot(target=target, color=color, linewidth=2)
            # fill area representing the movable's width
            offset = movable.size * 0.5
            below = [pos - offset for pos in movable.path.positions]
            above = [pos + offset for pos in movable.path.positions]
            target.fill_between(movable.path.times, below, above, color=color, alpha=0.5)
            # fill area between lower and upper bound
            try:
                entry = self.entry[movable]
            except KeyError:
                continue
            lb, ub = entry.lb, entry.ub
            if lb is None or ub is None:
                continue
            times = sorted(set(lb.times + ub.times))
            del times[:max(0, len(movable.path.times) - 1)]
            if times[-1] == MAXFLOAT:
                times.pop()
            lb_pos = [lb.position_at(t) for t in times]
            ub_pos = [ub.position_at(t) for t in times]
            target.fill_between(times, lb_pos, ub_pos, color=color, alpha=0.25)
            target.plot(times, lb_pos, linestyle="--", color=color)
            target.plot(times, ub_pos, linestyle=":", color=color)
            
        target.grid()
        # show the walls
        max_t = 0
        for movable in self.sequence:
            max_t = max(max_t, movable.path.time)
            try:
                entry = self.entry[movable]
            except KeyError:
                continue
            if entry.lb and entry.ub:
                max_t = max(max_t, entry.lb.times[-2], entry.ub.times[-2])
        for wall in (self.limits.lb, self.limits.ub):
            target.plot([0, max_t], [wall.position, wall.position], 
                        color="black", linestyle="dashed")
        if title is not None:
            target.set_title(str(title))
        target.figure.show()
        return target
        
        
class BoundsEntry(object):
    """Associates a Movable1D with the lower and upper bounds on its movement."""
    def __init__(self, movable):
        self.movable = movable
        self.lb = None
        self.ub = None
        
    @property
    def bounds(self):
        return (self.lb, self.ub)
        
    def compute_lb(self, entry_below, force=False):
        if force or self.lb is None:
            lb_limit = entry_below.lb.deepcopy(copy_segment_cache=False)
            lb_limit.offset_position((self.movable.size + entry_below.movable.size) * +0.5)
            self.lb = compute_bound(self.movable.path.deepcopy(), lb_limit, 
                                    direction=-1, speed=self.movable.speed)
            
    def compute_ub(self, entry_above, force=False):
        if force or self.ub is None:
            ub_limit = entry_above.ub.deepcopy(copy_segment_cache=False)
            ub_limit.offset_position((self.movable.size + entry_above.movable.size) * -0.5)
            self.ub = compute_bound(self.movable.path.deepcopy(), ub_limit, 
                                    direction=+1, speed=self.movable.speed)
            
    def out_of_bounds(self, time, position):
        return (compare.lt(position, self.lb.position_at(time)) or 
                compare.gt(position, self.ub.position_at(time)))
        
        
def compute_bound(bound, limit, direction, speed):
    """Given a (partially constructed) bound, a movement direction (+1 for up/right, -1 for 
    left/down), maximum movement speed, and a limiting Path object, complete the current bound 
    so that it "sticks" to the limit as much as possible while respecting the movement speed and 
    avoiding crossing the limit."""
    while compare.lt(bound.time, limit.time):
        index = limit.segment_indices_at(bound.time)[-1]
        segment = limit.segment(index)
        if (compare.neq(bound.position, segment.y_at(bound.time)) or 
            direction > 0 and compare.gt(segment.m, +speed) or
            direction < 0 and compare.lt(segment.m, -speed)):
            _approach_limit(bound, limit, direction, speed)
        elif compare.leq(abs(segment.m), speed):
            _follow_limit(bound, limit, direction, speed, index, segment)
        else:
            _avoid_limit(bound, segment, direction, speed)
    assert compare.eq(bound.position, limit.position)
    return bound
    
    
def _approach_limit(bound, limit, direction, speed):
    """Approach the limiting Path at full speed."""
    assert compare.leq(direction * bound.position, direction * limit.position_at(bound.time))
    approach = Line.from_point(bound.time, bound.position, speed * direction, open_left=False)
    for (time, position), segment in limit.intersections(approach):
        if compare.gt(time, bound.time):
            bound.append(time, position)
            return
    #raise Exception("hey! you're not supposed to be here!")
    # yet another dirty fix for the shitty floating point arithmetic errors...
    segment = limit.segments_at(bound.time)[-1]
    time, _ = bound.pop()
    bound.append(time, segment.y_at(time))
    
    
def _follow_limit(bound, limit, direction, speed, index, segment):
    """Follow the limit Path while the slope of its segments is less than or equal to the maximum 
    movement speed specified.""" 
    while compare.leq(abs(segment.m), speed):
        bound.append(segment.x1, segment.y1)
        index += 1
        if index < len(limit) - 1:
            segment = limit.segment(index)
        else:
            # limit.segment(i) requires both limit.times[i] and 
            # limit.times[i+1], so i must be less than len(limit) - 1
            break
            
            
def _avoid_limit(bound, segment, direction, speed):
    """The object is unable to follow limit (we only know about the limit's next segment), so we 
    must compute a trajectory that avoids crashing into the limit and merge it back with the 
    bound that we built up to this point."""
    assert compare.eq(bound.position, segment.y_at(bound.time))
    avoid = Line.from_point(bound.time, bound.position, speed * -direction)
    dx = avoid.x_at(segment.y1) - segment.x1
    #assert compare.gt(dx, 0.0)
    if compare.eq(dx, 0.0):
        # theoretically, dx should be greater than zero here, but since we're working with floating 
        # point numbers, there are cases where this inequality does not hold and this if statement
        # presents a quick'n'dirty fix for those cases. The alternative is to eventually convert 
        # all floating point computations to the decimal module or something similar...
        bound.append(segment.x1, segment.y1)
        return
    avoid = Line(bound.time - dx, bound.position, segment.x1, segment.y1)
    intersection = bound.intersection(avoid, index=-1)  # we want the right-most intersection
    if intersection is None:
        raise Exception("impossible to avoid neighbor bound (infeasible state)")
    (time, position), _ = intersection
    while bound.time > time:
        bound.pop()
    bound.append(time, position)
    bound.append(segment.x1, segment.y1)
    
